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ABSTRACT 


The smooth variable structure filter (ASVSF) has been relatively considered as a new 
robust predictor-corrector method for estimating the state. In order to effectively utilize 
it, an SVSF requires the accurate system model, and exact prior knowledge includes 
both the process and measurement noise statistic. Unfortunately, the system model is 
always inaccurate because of some considerations avoided at the beginning. More- 
over, the small addictive noises are partially known or even unknown. Of course, this 
limitation can degrade the performance of SVSF or also lead to divergence condition. 
For this reason, it is proposed through this paper an adaptive smooth variable struc- 
ture filter (AS VSF) by conditioning the probability density function of a measurement 
to the unknown parameters at one iteration. This proposed method is assumed to ac- 
complish the localization and direct point-based observation task of a wheeled mobile 
robot, TurtleBot2. Finally, by realistically simulating it and comparing to a conven- 
tional method, the proposed method has been showing a better accuracy and stability 
in term of root mean square error (RMSE) of the estimated map coordinate (EMC) and 
estimated path coordinate (EPC). 
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1. INTRODUCTION 


The presence of a map can be useful for a mobile robot to perform its navigation task. Unfortunately, 
the consistent map is unavailable at the beginning [1]. Consequently, the system should be able to track the 
robot path and to build a map based on the robot measurement [2-4]. It can be done by knowing the current 
pose when sensing a feature and locating the pose based on the around features [5]. Since these tasks should 
be addressed at the same time, the definition of simultaneous localization and mapping (SLAM) is stated [6- 
12]. The SLAM-based mobile robot navigation has intensively received attention because of some challenging 
factors that need to be solved such as wide uncertainty, system complexity, inaccurate system model, limited 
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prior information, noise statistics of the process and measurement, computational cost and filter divergence [13, 
14]. 

Additionally, in the mobile robot application, the successful of solving SLAM problem can be val- 
idated root mean square error (RMSE) [15-17] calculated based on the different of the estimated and true 
value. The continuosly grown uncertainty makes the estimated values deviates from its base. For this reason, 
the probability-based filtering method has been intensively used. It has been frequently adopted to effectively 
represent all the possibility related to the system [18]. The most popular one is extended kalman filter which 
has the basic task to update the state and covariance with an assumption all the related variable comply with 
Gaussian distribution. Generally, extended kalman filter [11, 19-23] is known as an nonlinear version of its pre- 
decessor named kalman filter [18-20, 24-28]. The easiness to apply extended kalman filter makes it has been 
widely used to solve in many different fields such as for the state and parameter estimation including SLAM, 
signal processing, fault detection and diagnosis and target tracking [29]. Nevertheless, it has many incompat- 
ibilities and difficulties such as the deviant solution from the state trajectory, less optimal state estimation and 
large estimation error due to the linearization process and computational cost [14, 15, 17]. These limitations 
make its practical application becomes limited nowadays. 

Furthermore, many researcher have switched to use the similar method that might offered better ro- 
bustness rather than EKF such as unscented kalman filter (UKF) [20, 30-32], cubature kalman filter (CKF) [15, 
16, 26] smooth variable structure filter (SVSF) [15-17, 33], etc. Their recorded successes have been proving 
to have significant improvement of EKF. Among of them, SVSF has been rapidly developed. The SVSF is a 
relative new predictor-estimator, which is first invented in 2007 [15, 16]. Firstly, it was derived from a variable 
structure filter (VSF)[34] and extended variable structure filter (EVSF) [13, 17, 35]. Then proceed with a pres- 
ence of new form by completing it with the error covariance matrix without affecting its accuracy and stability 
[35, 36]. As a common filtering technique, it was then enhanced by involving the time-varying boundary layer 
width to replace its previous characteristic [36, 37]. Due to these developments, SVSF becomes the popular 
method to against the uncertainty which is not only suitable for the linear but also nonlinear system [15-17]. 
Additionally, based on its characteristic, the SVSF can be combined with different methods in obtaining the 
optimal solution [15-17, 33, 34]. However, like the other traditional filter methods, to apply SVSF the noise 
statistic is often predefined to be fixed and constant for the whole estimation process. It often leads to diver- 
gence and degradation performance. Thus, traditional SVSF should at least be enhanced to partially estimating 
the noise statistic. Therefore, through this paper, SVSF is adaptively improved. Firstly, the SVSF’s error covari- 
ance matrix is mathematically derived via maximum a likelihood estimator [38]. It aims to recursively update 
the process covariance matrix Q as well as the measurement error covariance matrix R. Since the prediction 
step of SVSF [15, 16, 33, 34] is totally same as EKF, so that, the innovation error can be similarly computed. 
Consequentially, the derivation process of finding the compact formulation of Q and R is easy to be evaluable 
[31, 38]. Additionally, since this algorithm is used to solve SLAM problem, henceforth it is termed as AS VSF- 
SLAM algorithm in this paper. In case of knowing the performance of this algorithm, the proposed algorithm 
is realistic simulated and compared with traditional SLAM algorithm. Based on the comparative results, it can 
be declared that the ASVSF-SLAM algorithm can significantly solve the SLAM problem of wheeled mobile 
robot in term of RMSE for both estimated path coordinate and estimated map coordinate. 

The rest parts of this paper are arranged as follows. Section 2 presents a discussion of the original 
SVSF. Section 3 sequentially presents the mathematical derivation conducted to find the recursive error co- 
variance matrix of both the process and measurement noise statistic. Section 4 discuss an algorithm named 
ASVSF-SLAM algorithm with expansion of kinematic configuration and motion model, direct point-based ob- 
servation and, inverse point-based observation. Finally, Section 5 presents a conclusion according to the result 
discussed in previous section 


2. CLASSICAL SVSF 
Considering that the dynamic model of Gaussian nonlinear system is given as follows 


(1) 


te=] DRAG) OR 
Zk = h(x) + Vk 


where k is discrete time index, x € R” is the state vector, u is the control vector and z € R™ is the measurement 
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vector. While, w € R” and v € R™ are small process, and measurement noise, respectively. Whereas, f (.) 
and h (.) are the nonlinear function and measurement model, respectively. The statistical characteristic of this 
dynamic model is given as follows 


Ew] = 0, Cov|wk, wj] = Qkôkj 
E |v] = 0, Cov|Vk, Vj] = Rkôkj (2) 
E|wk, v;| = 0 


where ô is Kronecker delta function. Meanwhile, E |.| and Cov |, | are mean and covariance term, respectively. 
Then the complete formulation of SVSF can be chainned as follows 


tik =f Cpa a (3) 

Popa = FPP FOr (4) 

Cx hea =k = Ne) — Te (5) 

A = lez xin—1 |aba + Teakele labs (6) 

y= (A HP yp 1H? (Hyp HT +R) (7) 


Loy eak e] 


sati enre- = 4 aAa a —l<v ea <1 (8) 
—1 , pape £ —1 
KSVSEF — H*{A O sath tez rk] } lezka] (9) 
k Coa ER ees (10) 
Pri = QU — HK Jezik nea =HK AT FR T RRK OP (11) 
ez kik = Zk — h(ĉkjk) — Tk (12) 
eka apse lezka (13) 


3. ESTIMATING THE NOISE STATISTIC 

Traditionally, SVSF has no ability to approximate the responsive noise statistic. Consequently, the 
possibility of divergence still high in case of either the realistic simulation or real application. Moreover, 
inaccurate modelled system might also increase the uncertainty that is precisely lead to degradation of filter 
performance. For this reason, an effort to complete SVSF with recursive noise statistic is proposed in this 
paper. This process can be done by estimating error matrix of the noise statistic by deriving the predicted error 
covariance matrix of the state using MLE [31, 38]. 

First, by recalling the innovation sequence of SVSF and considering that the nonlinear system (1) 1s 
Gaussian. According to [38, 39] and [6], Są can also be alternatively calculated as 


C= > dd} (14) 
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where dą is innovation sequence error as shown in (5), Ch is alternative form of Sg, and the addition of 
ee k-N4+1 dj di is the moving window [40] for N refers to the window size. 

Now, by recalling as shown in (2) and assuming that the unknown parameter a = {Q, R}, the adapta- 
tion process is done with assumptions that State vector x is not depend on a 1.e oe F and H are independent 
of a and time variant, €% is white and ergodic sequence within the estimation window, and S% is regarded as the 
main key of adaption on depend parameters. Therefore, the probability density function of the measurements 
conditioned on the parameter a at time k can be described as follows [31, 38] 


1 
Pooja), = ((27)" Ck?) exp ( - 5 lldzlloz: ) (15) 


Then by taking its algorithm, and ignoring all the constants, the compact formulation of shown in (15) is 


k 
Ja) = ys di Cr d; = min (16) 
j=k-N+1 


Next, by addopting two relations of matrix differential calculus [38], (16) is derived as follows 


k 
J@= J- eter) A A = min (17) 


O 
j=k-N+1 


At this point, it is clear that the adaptive SVSF lies on the determination of C and its partial derivative 
with respect to œ. Additionally, the main interest is in Ry, and Q, instead of C [31, 41, 42] . Therefore, by 
sequentially referring to (6), first derivative of C with respect to a, and a condition of P,_1),—1 in the steady 
state, (17) can be compactly expressed as follows 


k 
` md OC =C da] ap -+ al = 0 (18) 


J Oa Oa 
j-k—-N+41 


At this point, the process of obtaining both the adaptive estimation of the process noise and measure- 
ment noise covariance are presented. First, Q is assumed to be known and independent of a to obtain the 
explicit expression for R. Similarly, the process to gain the expression for Q will also involve the assumption 
that R is fixed and independent of a at the previous process. Both processes can be sequentially described at 
the following subsection 


3.1. Adaptive covariance matrix of process noise statistic 
Given K then (18) becomes 


k 
DD tr] HOS MH” - HC; ta,d) CHT =0 (19) 
j=k-N+1 


Then by referring to (7) after replacing Sp by Cp, the alternative formulation of C% ' is obtained as follows 


Cy'HT = age A al aay (20) 


Since, WAH T is gain of SVSF on (9), and the saturation function on (8) is satisfied at the previous step, 
then it is clear to have the following form 
a? SVSF p-1 
C, H = kK; Pkt (21) 


Since (21) is formed then by substituting (21) into (19), it yields 
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k 
DD ata [K?VSF HP; — e KSVSFT |p| =0 (22) 
j=k—N+1 


where P; is Pkjk—1. By definition, it should be at least semi-definite positive matrix, thus a can be vanished. 


k 
>» tr] KiVSPHP, — KSVSFdsat KEYS) =0 (23) 
j=k-N+1 


As well-known that the alternative formulation of (11) is Pjk = Pyy,—1 — HKV ST Ppjk—1, thus 
Pie — Pee =i FHP (24) 
Now, substituting (10) and (24) into (23), it yields 


k 

D md P} =P= [6] = 4) )@ = a =0 (25) 
j=k—N+1 

Note that P™ and P- refer to Py, and Pkjk—1, respectively. Meanwhile, 2T and £~ refer to ĉ k|k and Êkjk—1, 
respectively. Then by substituting (4) into (25), it yields 


k 
5o Pt —-FPtFT =Q- r-a e a] (26) 
j=k-N+1 
Finally, the covariance matrix of process noise statistic is obtained 
k 
Q= S d -L Peso = e (27) 
j=k-N+1 


Suppose that (24) alternatively represents the updated covariance of SVSF. It is totally same with Kalman 
Filter, then (26) can be approximately reformed as 


A T 
Q = Beer Cie (28) 


3.2. Adaptive covariance matrix of measurement noise statistic 
Similarly, since Q is fixed and independent of a, then (18) becomes 


k 
=i —1 4 9T 7-1 
DD d [Cj — C; djd; C} Jo+]}=0 (29) 
j=k-N+1 
It can also be simplified as 
k 
pp troy lc, — isd? ost} =0 (30) 
jg=k—-N+1 
then by deriving (30), it yields 
k 
1 
Ch=q ` dd; (31) 
j=k-N+1 


Now since = > j=k—N+1 jd}; is the way to calculate the expectation of d;d; , which is also as shown in (2), 
then the recursive measurement error covariance matrix 1s 


R= Cy =H Pyra H" (32) 


Mathematical derivation above showing that the adaptive covariance matrix of measurement noise statistic 
seems able to be adopted from the value used at the previous iteration. Finally, both the process and measure- 
ment noise statistic are obtained already when their corresponding mean are considered to be zero. 
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4. ADAPTIVE SVSF-BASED FEATURE 2D SLAM ALGORITHM 

The proposed method is approached to address a feature-based SLAM problem of the wheeled mobile 
robot [14, 17]. The objective is concurrently estimating the robot pose and feature in certain environment. It is 
assumed that by using the motion model the robot moves after executing the control command, and by using 
laser scanner-based measurement, it senses the features with distance and bearing as the gathered data [3, 4, 
43, 44]. It is noted that all the prerequisites for a feature-based SLAM algorithm in [14] and [17], therefore, 


Algorithm 1 ASVSF-SLAM Algorithm 

1. Loop 

2. Prediction Step: If proprioceptive data is available 
3. Propagate the state estimate ((3)) 

4. Propagate the covariance relative to the state ((4)) 
5. Update Step: If the observation data is available 
6. Compute the innovation sequence error ((5)) 

7. Calculate Gain ((9)) 

8. Update the State, and Covariance ((10) and (11)) 
9. Compute the noise statistic ((28) and (32)) 

10. endloop 


5. RESULTS AND DISCUSSION 

The proposed algorithm discussed above is realistically simulated and applied for solving SLAM 
problem of wheeled mobile robot. Initially, the robot pose and covariance as well as all the completeness 
parameters for ASVSF are initialized as follows 


ea paa] isa 

ĉo= | 0 |, Pa = |0 0 0| ,y=15e—2 , ezo = |0.1 —— (33) 
a ooo Pea 
180 


Note that all the parameters shown herein are adopted from the real robot platform, Turtlebot2, which equipped 
with laser scanner [4, 43, 14] in displacement dıs = 14cm. By realistically measuring the distance between two 
separately powered wheels, it is obtained W, = 33cm. It assumed that this mobile robot is operated in planar 
indoor environment with the size and random point as described in Figure 1 that is served as the reference path 
and map in this experiment. The validation is conducted based on RMSE of different algorithm in estimating 
the robot path and map. 








. Real Trajectory 
* Real Landmark Position 
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Figure 1. Reference trajectory and map 
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The validation is conducted based on RMSE of different algorithm in estimating the robot path and 
map. There are two different condition of the initial noise statistic in order to see the consistency of the proposed 
algorithm as shown in Table 1. All the parameters presented in Table 1 aim to know the performance of the 
proposed algorithm when the initial process and measurement noise statistic are increased in type of additive 
white Gaussian noise. 


Table 1. The predefined noise statistic 


No. fo Ro ĝo Qo 
E an 0.5 0.57 0 0.05 0.05 0 
57/180 0 52/180? 27/180 0 27/1807 
0.8 0.8? 0 0.1 0.1? 0 
nd ç; : 
een an k m wN K A 


All the parameters presented above aim to know the performance of the proposed algorithm when 
the initial process and measurement noise statistic are increased in type of additive white Gaussian noise. The 
scenario is involved to validate the stability of the proposed ASVSF-SLAM algorithm compared to conven- 
tional SVSF-SLAM algorithm. Regarding to these parameters and the reference path depicted in Figure 1 the 
following results were obtained. It analogized that the robot moves based on all the command send to the 
odometers. It aims to track the reference path as well as locating new detected landmark in the environment 
then by applying two different algorithm which are SVSF-SLAM and Adaptive SVSF-SLAM algorithm, the 
performance of the robot can be graphically expressed as shown in Figure 2. 


Simulation Result Simulation Result 


























Tea True Path * » True Path 
— * True Landmark ” x P * True Landmark 
* j «, [77 7SVSF-SLAM Path . * | [>> 7 SVSF-SLAM Path 
"es 7 * SVSF-SLAM Landmark . J * SVSF-SLAM Landmark 
200 + on - - -ASVSF-SLAM Path 200 + E Z z - - -ASVSF-SLAM Path 
j * ASVSF-SLAM Landmark 














Sy _| * ASVSF-SLAM Landmark 
a 


* 





ne 
Ngee eee 


is (cm) 


-` 
- ~ 
s 

















-300 L | i L i j -300 i l j N n 
-100 0 100 200 300 400 500 -100 0 100 200 300 400 


(a) (b) 
Figure 2. Performance of SVSF and ASVSF-SLAM algorithm for (a) Ist simulation and (b) 2nd simulation 


Figure 2 represents the performance of different SLAM algorithm. They are attempted to estimate 
the path and map coordinate by respecting to the reference trajectory. According to different simulations, 
both SVSF and ASVSF-based SLAM algorithm show the convergence to track the reference. Additionally, 
the proposed method shows a smoother performance with a guaranteed stability when the noise statistic is in- 
creased. However,it is quite difficult to know the detail diversity of the SLAM algorithms through the graphical 
representation only. 

Therefore, to ease the validation and analysis, their estimated path and estimated map coordinate are 
compared in term of root mean square error. Figure 3 depicts the difference RMSE values for SVSF and 
ASVSF-SLAM algorithms. According to two different simulation, it is clear to see that the adaptive SVSF 
gives smaller RMSE for the estimated robot path in which there is no much diversity to its reference. By 
using the same method and term, the estimated map of SVSF and ASVSF-SLAM algorithm is also compared. 
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According to Figure 4 the stability and accuracy of ASVSF-SLAM is validated. There is no significant effect 
after increasing the initial noise statistic. Therefore, it can be stated that since no degradation detected, ASVSF- 
SLAM algorithm is better than SVSF-SLAM algorithm. 
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(a) (b) 
Figure 4. RMSE of estimated path coordinate (a) Ist Simulation and (b) 2nd Simulation 


Referring to all the graphical result above, it might be difficult to see the difference. For this reason, 
the following Tables 2 and 3 are presented. In which, these tables are intended to give detail values for all test 
as the way to validate the accuracy for each algorithm. 


Table 2. RMSE of different SLAM-based algorithm (first test) 
SLAM Alg. x-EPC (cm) y-EPC (cm) 6-EPC (rad) x-EMC (cm) y-EMC (cm) 
SVSF 6.4992 11.2050 0.1066 16.1687 20.0962 
ASVSF 5.6514 2.6893 0.0991 11.1031 12.1210 


Table 3. RMSE of different SLAM-based algorithm (second test) 
SLAM Alg. x-EPC (cm) y-EPC (cm) 6-EPC (rad) x-EMC (cm) y-EMC (cm) 
SVSF 11.3148 19.2975 0.7886 31.3384 38.7499 
ASVSF 5.5325 6.4678 0.1109 24.9880 29.7186 


6. CONCLUSIONS 

The main contribution of this paper is to equip the traditional SVSF with an approach termed as 
adaptive filtering strategy. It utilizes the maximum likelihood estimation (MLE) used to approximate the error 
covariance matrices of noise statistic by conditioning the probability density function of measurement to an 
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unknown parameter at one iteration. The output of this project is the enhancement of SVSF. It can recursively 
calculate the covariance of the noise statistic based on the uncertainty condition of the previous process. In other 
words, the predetermined covariance of the noise statistic is executed once at the first iteration and the system 
continuously generates new covariances for the rest iteration. The proposed method is applied to solve the 
feature-based SLAM problem of a wheeled mobile robot named ASVSF-based SLAM algorithm. The presence 
of adaptive strategy prevent the SVSF from degradation and divergence condition under time integration when 
it is applied for the real case. Regarding to all the comparative results, the accuracy, stability, and robustness of 
the proposed algorithm is guaranteed and satisfied. 
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